

public class WallFollow extends Proportional {

	
	public WallFollow() {
		// initializing with 1 to prevent division with 0
		m_highestVal = 1;
		setWaitTime(5);
	}

	public void doWork() throws Exception {
		//getting sensor values
		int s0 = getDistanceValue(0);
		int s1 = getDistanceValue(1);
		int s2 = getDistanceValue(2);
		int s3 = getDistanceValue(3);
		int s4 = getDistanceValue(4);
		int s5 = getDistanceValue(5);
		
		//getting rough values for left and right motor speed
		int aL = s0 + s1;
		int aR = s2 + s3 + s4 + s5;
		//Searching for biggest speed value
		if (aL > m_highestVal) {
			m_highestVal = aL;
		};
		//scaling the speed value between 0 and 5		
		int speedL = (5 * aL) / m_highestVal;
		int speedR = (5 * aR) / m_highestVal;
		
		//setting speed that robot doesn't starve and stands still
		if (speedL < 3) {
			speedL = 3;
		}
		// if front and right sensors don't realize wall robot goes on, otherwise turns
		if (speedR == 0) {
			speedR = speedL;
		} else {
			speedR = 1 - speedL;
		}
		
		this.setMotorSpeeds(speedL, speedR);

	}

	public void close() throws Exception {
	}
}
